/*
出于简单和方便大家模拟。 本例子采用和前面飞机起飞爬升的例子。
但又有一些不同(假设飞机保持固定偏航角pitch起飞爬升，使用了固定加速度：a=20米/s^2 和 固定俯扬角：θ=30°。  
数据融合系统开始工作起始速度： 70米/s。 因为飞机达到起飞速度才离地起飞)：
1。在EKF实现的例子中，只是简单的根据三个传感器数据融合了飞机起飞爬升阶段海拔高度。
    本例中将同时计算水平前进距离：x,海拔高度：h,以及沿俯扬角的前进速度：v，即：系统状态值X（x，h,v）；
2。因为飞机爬升成非线性，本例采用UKF来实现；


同样，飞机上安装有三个传感器:
1.GPS定位系统,可以读取一个带有噪声的前进位移和海拔高度值: d_gps 和 h_gps 单位: 米;
2.气压计(barometers), 可以读取一个带有噪声的受温度和高度影响的气压值: p_bp 单位: kPa;
3.另外还有一个速度计（Speedometer），获取一个带有噪音的速度值: v_speed 单位: 米.
这样，我们的测量值列向量共4个： z_k(d_gps,h_gps,p_bp,v_speed)

****************************************************************************
实际工作中往往是根据测得的温度和压强垂直分布去计算位势高度，因几何高度难以直接测量。
大气压同海拔高度的关系：
　　P=P0×（1-H/44300)^5.256
计算高度公式为:
   H=44300*(1- (P/P0)^(1/5.256) )
式中：H——海拔高度，P0=大气压（0℃，101.325kPa）
**************************************************************************

一。系统状态和测量值分析
系统状态列向量X_k: (d, h, v)     n=3
系统控制有： a = 20米/s^2，  俯扬角：θ=30°
系统初始速度为： 70米/s。 因为飞机达到这个速度才离地起飞， x 和 h值也从此时开始计算。
系统测量值列向量Z_k: (d_gps,h_gps,p_bp,v_speed)  m=4
二。系统运动学方程
同KF公式（1）： X_k = F_k * X_k-1 + B_k * u_k
根据初中物理运动学公式:
vt = v0 + a * Δt
st = s0 + v0 * Δt + 0.5 * a * (Δt)^2
考虑固定加速度：a=20米/s^2  和 到固定俯仰角： θ=30°
得系统运动学状态转移模型为：
水平方向新位置x:     x_k =  x_k-1      +      0      +    cos(30°)*v_k-1*Δt       +    0.5*cos(30°) * a * (Δt)^2
垂直方向新位置h:     h_k =    0        +    h_k-1    +    sin(30°)*v_k-1*Δt       +    0.5*sin(30°) * a * (Δt)^2
沿固定俯仰角的速度v:  v_k =    0        +      0      +           v_k-1            +                 a * Δt
其： F_k:
|  1  0  cos(30°)*Δt |      | 1  0  0.5*√3*Δt |
|  0  1  sin(30°)*Δt |  =   | 0  1   0.5*Δt   |
|  0  0       1      |      | 0  0      1     |
列向量B_k:  (省略u_k， 因为a固定值，所以它和B_k合并)
| 0.5*cos(30°) * 20 * (Δt)^2 |    |   5√3 * (Δt)^2  |
| 0.5*sin(30°) * 20 * (Δt)^2 |  = |     5 * (Δt)^2  |
|            20 * Δt         |    |     20 * Δt     |

其他：
1。 因为本例基于简化UKF滤波算法（加性噪声）模型实现。 所有Q_k和R_k也是单独一个矩阵，
2。 UKF滤波算法中没有明确的状态值到测量值转移矩阵H_k, 所以不定义,用h(x)替换；
3。 UKF滤波算法中，对卡尔曼增益K 和 系统不确定性协方差矩阵P_k的计算 与 KF和EKF不同。不能直接套用卡尔曼滤波器五大公式；
*/
#include "UKF.h"

using namespace std;

#define  DEBUG (false)

#define PI (3.141592653589793)

namespace ukf
{
    /*
    Note： 本例基于简化UKF滤波算法（加性噪声）模型实现
    另有： 扩维UKF滤波算法（噪声隐含）   大致算法类似
    */

    UKF::UKF(int _stateCount, int _measurementCount)
    {
        stateCount = _stateCount;
        measurementCount = _measurementCount;

        lambda = std::pow(ALPHA, 2) * (stateCount + K) - stateCount; // 在UKF公式中： k为第二个尺度参数，通常设置为3或3-n。  n为状态向量维数，或状态值个数
        lambda_plue_n_x_sqrt = std::sqrt(lambda + stateCount);       //√(λ+n)   一个常用中间值
        sigmaPointsCount = 2 * stateCount + 1;                          // Sigma points count

        weightVector.resize(sigmaPointsCount);
        weightVector = 0;
        //初始化各sigma points的权值
        double weight_0 = lambda / (lambda + stateCount);//W_0 = λ / (n + λ) 
        weightforVariance4PointOne = weight_0 + (1- std::pow(ALPHA, 2) + BETA); // λ / (n + λ)  + (1 - a^2 + β)

        weightVector(1) = weight_0; //W_0 = λ / (n + λ) 
        for (int i = 2; i <= sigmaPointsCount; i++)
        {   //其他2n个sigma points‘ weights
            double weight = 0.5 / (stateCount + lambda); //W_i = 1 / 2(n + λ) i = 1,2, 3, ..., n
            weightVector(i) = weight;
        }
#if (DEBUG)
        cout << "UKF::UKF====weightVector: " << weightVector << endl;
#endif

        X_k.resize(stateCount);
        X_k = 0.0;

        //UKF中P_k根据sigma points计算出来的,是个先验估计值， 故没有初始值
        P_k.resize(stateCount);
        P_k = 0.0;

        //最优估计值,或上一时刻最优估计值. 如分析描述，  系统初始速度为： 70米/s。 因为飞机达到这个速度才离地起飞， x 和 h值也从此时开始计算。
        X_post.resize(stateCount);
        X_post(1) = 0.0; // d
        X_post(2) = 0.0; // h
        X_post(3) = 70.0;// v

        //最优协方差矩阵,或上一时刻最优协方差矩阵
        P_post.resize(stateCount);
        P_post = 0.0;
        for (unsigned int i = 1; i <= stateCount; i++)
            P_post(i, i) = 1;
        P_post *= 1;//默认协方差，

        //F_k依赖Δt，后面动态计算F_k(1, 3)和F_k(2, 3)
        F_k.resize(stateCount);
        F_k = 0.0;
        for (unsigned int i = 1; i <= stateCount; i++)
            F_k(i, i) = 1;

        //B_k依赖Δt，后面动态计算
        B_k.resize(stateCount);
        B_k = 0.0;

        Q_k.resize(stateCount);
        Q_k = 0.0;
        for (unsigned int i = 1; i <= stateCount; i++)
            Q_k(i, i) = 1;
        Q_k *= 1e-4; //预测过程噪音值。 这个值填一个较小的值即可

        R_k.resize(measurementCount);
        R_k = 0.0;
        R_k(1, 1) = 0.0000001; // GPS噪音 -> d
        R_k(2, 2) = 0.0000001; // GPS噪音 -> h
        R_k(3, 3) = 0.00001;     // 气压计噪音噪音 -> h
        R_k(4, 4) = 0.00000001; // 速度计噪音噪音  -> v


        //单位矩阵，可能没用
        Identity.resize(stateCount);
        Identity = 0.0;
        for (unsigned int i = 1; i <= stateCount; i++)
            Identity(i, i) = 1;
    }

    UKF::~UKF()
    {
    }

    void UKF::UpdateRK(SymmetricMatrix &r)
    {
        R_k = 0.0;
        R_k(1, 1) = r(1, 1); // GPS噪音 -> d
        R_k(2, 2) = r(2, 2); // GPS噪音 -> h
        R_k(3, 3) = r(3, 3);     // 气压计噪音噪音 -> h
        R_k(4, 4) = r(4, 4); // 速度计噪音噪音  -> v
    }

    //仅动态计算F_k(1, 3)和F_k(2, 3)，其他保持不变
    void UKF::UpdateFK(double deltaT)
    {
        static double _SquareRootValue = 0.5 * std::sqrt(3); //cos(30°)= √3 / 2

        //值的来历， 请参见文件头的状态分析
        F_k(1, 3) = _SquareRootValue * deltaT; //cos(30°)*Δt
        F_k(2, 3) = 0.5 * deltaT;              //sin(30°)*Δt
    }

    //动态计算B_k
    void UKF::UpdateBK(double deltaT)
    {
        static double _SquareRootValue2 = 5 * std::sqrt(3); // 1/2 * cos(30°) * 20 = 1/2 * (√3 / 2) * 20

        //值的来历， 请参见文件头的状态分析
        B_k(1) = _SquareRootValue2 * std::pow(deltaT, 2); //0.5*cos(30°) * 20 * (Δt)^2
        B_k(2) = 5 * std::pow(deltaT, 2);                 //0.5*sin(30°) * 20 * (Δt)^2
        B_k(3) = 20 * std::pow(deltaT, 2);                //20 * Δt
    }

    void UKF::GetPostEstimate(ColumnVector &x_post /*out*/) //, SymmetricMatrix &p_post /*out*/)
    {
        x_post = X_post;
        //p_post = P_post;
    }

    void UKF::PredictAndUpdate(double deltaT, ColumnVector &z /*in*/)
    {
        UpdateFK(deltaT);

        UpdateBK(deltaT);

        //create sigma point matrix
        Matrix Xsig = Matrix(stateCount, sigmaPointsCount);

        //Step 1
        Step1_GenerateSigmaPoints(X_post, P_post, Xsig);
#if (DEBUG)
        cout << "Step1_GenerateSigmaPoints: Xsig=\n[\n";
        for (unsigned int row = 1; row <= stateCount; row++)
        {
            for (unsigned int col = 1; col <= sigmaPointsCount; col++)
            {
                cout << Xsig(row, col) << ",";
            }

            cout << endl;
        }
        cout << "]"<<endl;
#endif

        //create predict sigma point matrix
        Matrix Xsig_pred = Matrix(stateCount, sigmaPointsCount);

        //Step 2
        Step2_PredictSigmaPointsByStateFunction(Xsig, Xsig_pred, deltaT);
#if (DEBUG)
        cout << "Step2_PredictSigmaPointsByStateFunction: Xsig_pred=\n[\n";
        for (unsigned int row = 1; row <= stateCount; row++)
        {
            for (unsigned int col = 1; col <= sigmaPointsCount; col++)
            {
                cout << Xsig_pred(row, col) << ",";
            }

            cout << endl;
        }
        cout << "]" << endl;
#endif

        //Step 3
        Step3_CalculateMeanAndCovarianceByWeight(Xsig_pred, X_k, P_k);
#if (DEBUG)
        cout << "Step3_CalculateMeanAndCovarianceByWeight: X_k=\n(\n";
        for (unsigned int row = 1; row <= stateCount; row++)
        {
            cout << X_k(row) << ",";

            cout << endl;
        }
        cout << ")" << endl;
#endif

        //Step 4
        Matrix Zsig_pred = Matrix(measurementCount, sigmaPointsCount);
        Step4_PredictSigmaPointsByMeasurementFunction(Xsig_pred, Zsig_pred);
#if (DEBUG)
        cout << "Step4_PredictSigmaPointsByMeasurementFunction: Zsig_pred=\n(\n";
        for (unsigned int row = 1; row <= measurementCount; row++)
        {
            for (unsigned int col = 1; col <= sigmaPointsCount; col++)
            {
                cout << Zsig_pred(row, col) << ",";
            }

            cout << endl;
        }
        cout << ")" << endl;
#endif

        //Step 5
        ColumnVector ZZ_k(measurementCount);
        ZZ_k = 0.0;
        Matrix ZZP_k(measurementCount, measurementCount);
        ZZP_k = 0.0;
        Step5_CalculateMeasurementMeanAndCovarianceByWeight(Zsig_pred, ZZ_k, ZZP_k);
#if (DEBUG)
        cout << "Step5_CalculateMeasurementMeanAndCovarianceByWeight: ZZ_k=\n(\n";
        for (unsigned int row = 1; row <= measurementCount; row++)
        {
            cout << ZZ_k(row) << ",";

            cout << endl;
        }
        cout << ")" << endl;
#endif

        //Step 6
        Step6_UpdatePostState(Xsig_pred, X_k, P_k,
                                     Zsig_pred, ZZ_k, ZZP_k,
                                     z, X_post, P_post);
#if (DEBUG)
        cout << "Step6_UpdatePostState: X_post=\n(\n";
        for (unsigned int row = 1; row <= stateCount; row++)
        {
            cout << X_post(row) << ",";

            cout << endl;
        }
        cout << ")" << endl;

        cout << "Step6_UpdatePostState: P_post=\n(\n";
        for (unsigned int row = 1; row <= stateCount; row++)
        {
            for (unsigned int col = 1; col <= stateCount; col++)
            {
                cout << P_post(row, col) << ",";
            }

            cout << endl;
        }
        cout << ")" << endl;
#endif
    }

    //Matrix Xsig = Matrix(stateCount, sigmaPointsCount);  //create sigma point matrix
    void UKF::Step1_GenerateSigmaPoints(ColumnVector& x /*X_post*/, SymmetricMatrix& p /*P_post*/, Matrix &Xsig)
    {
        //calculate square root of P
        for (unsigned int row = 1; row <= stateCount; row++)
        {
            for (unsigned int col = 1; col <= stateCount; col++){
                p(row, col) = std::sqrt(p(row, col));
                //Note: 这里前一行sqrt时,可能出现-nan情况,   判断并设置为0.0
                if( isnan( p(row, col)) )
                    p(row, col) = 0.0;
            }

            //借用一下这个for循环，做点工作
            Xsig(row, 1) = x(row); //Xsig的第1列为x列向量本身
        }

        //设置其他2n个sigma点
        for (unsigned int row = 1; row <= stateCount; row++)
        {
            for (unsigned int col = 1; col <= stateCount; col++)
            {
                Xsig(row, col + 1) = x(row) + lambda_plue_n_x_sqrt * p(row, col); //Xsig的第2--n+1列
                Xsig(row, col + 1 + stateCount) = x(row) - lambda_plue_n_x_sqrt * p(row, col); //Xsig的第n+2--2n+1列
            }
        }
    }

    void UKF::Step2_PredictSigmaPointsByStateFunction(Matrix &Xsig, Matrix &Xsig_pred, double deltaT)
    {
        //按照系统状态方程（X_k = F_k * X_k-1 + B_k * u_k），更新2n+1个 sigma points
        for (unsigned int col = 1; col <= sigmaPointsCount; col++)
        {
            ColumnVector XsigCol_ = Xsig.columnCopy(col);
            ColumnVector XsigCol_2 = F_k * XsigCol_ + B_k; //类似KF/EKF公式（1）： X_k = F_k * X_k-1 + B_k * u_k
            for (unsigned int row = 1; row <= stateCount; row++)
            {
                Xsig_pred(row, col) = XsigCol_2(row);
            }
        }
    }

    void UKF::Step3_CalculateMeanAndCovarianceByWeight(Matrix &xsig_pred, ColumnVector &x_k, SymmetricMatrix &p_k)
    {
        //计算先验估计值： state mean
        x_k = (ColumnVector)(xsig_pred * weightVector); //x_k(nx1) = xsig_pred(nx(2n+1)) * weightVector((2n+1)x1)
        
        //计算先验协方差矩阵
        p_k = 0.0;
        for (int col = 1; col <= sigmaPointsCount; col++)
        {
            //各个sigma points相对于先验估计状态值x_k的距离
            ColumnVector x_diff = xsig_pred.columnCopy(col) - x_k; //x_diff(nx1)   x_k(nx1)
            //定义一个(stateCount, 1)矩阵，下面p_k的计算必须用矩阵，ColumnVector不可用
            Matrix x_diffMatrix(stateCount, 1);                    
            for (int row = 1; row <= stateCount; row++)
                x_diffMatrix(row, 1) = x_diff(row);

            //按照公式计算先验协方差矩阵
            if( col == 1 )  //对于第一个sigma points点,求其方差时,使用weightforVariance4PointOne
                p_k = p_k + (SymmetricMatrix)((Matrix)(weightforVariance4PointOne * x_diffMatrix) * x_diffMatrix.transpose()) + Q_k; //weightVector(i) * x_diff(nx1) * x_diff(1xn)
            else
                p_k = p_k + (SymmetricMatrix)((Matrix)(weightVector(col/*row*/) * x_diffMatrix) * x_diffMatrix.transpose()) + Q_k; //weightVector(i) * x_diff(nx1) * x_diff(1xn)
        }
    }

    void UKF::Step4_PredictSigmaPointsByMeasurementFunction(Matrix &xsig_pred, Matrix &zsig_pred)
    {
        //transform 2n+1 sigma points into measurement space
        //z_k(d_gps,h_gps,p_bp,v_speed)
        for (int col = 1; col <= sigmaPointsCount; col++)
        { 
            // extract values for better readibility
            double d = xsig_pred(1, col);
            double h = xsig_pred(2, col);
            double v = xsig_pred(3, col);

            // measurement model
            zsig_pred(1, col) = d; //d_gps   相等关系
            zsig_pred(2, col) = h; //h_gps   相等关系
            zsig_pred(3, col) = 101.325 * std::pow((1 - h / 44300), 5.256); //p_bp  <--  海拔高度转大气压  非线性关系
            zsig_pred(4, col) = v;//v_speed  相等关系
        }
    }

    void UKF::Step5_CalculateMeasurementMeanAndCovarianceByWeight(Matrix &zsig_pred, ColumnVector &zz_k, Matrix &zzp_k)
    {
        //计算测量值均值： state mean
        zz_k = (ColumnVector)(zsig_pred * weightVector); //zz_k(mx1) = zsig_pred(mx(2n+1)) * weightVector((2n+1)x1)

        //计算测量值协方差矩阵
        zzp_k = 0.0;
        for (int col = 1; col <= sigmaPointsCount; col++)
        {
            //各个sigma points相对于测量值zz_k的距离
            ColumnVector z_diff = zsig_pred.columnCopy(col) - zz_k; //z_diff(mx1)   z_k(mx1)

            //定义一个(measurementCount, 1)矩阵，下面zzp_k的计算必须用矩阵，ColumnVector不可用
            Matrix z_diffMatrix(measurementCount, 1);               
            for (int row = 1; row <= measurementCount; row++)
                z_diffMatrix(row, 1) = z_diff(row);

            //按照公式测量值协方差矩阵
            //Note:  zzp_k 这里建议都采用Matrix类型，因为掺杂着SymmetricMatrix类型会使对后面第6步矩阵求逆矩阵（inverse()）时，运算结果不正确
            zzp_k = zzp_k + ((Matrix)(weightVector(col /*row*/) * z_diffMatrix) * z_diffMatrix.transpose()) + (Matrix)R_k; //weightVector(i) * z_diff(mx1) * z_diff(1xm)
        }
    }

    void UKF::Step6_UpdatePostState(Matrix &xsig_pred, ColumnVector &x_k, SymmetricMatrix &p_k,
                                           Matrix &zsig_pred, ColumnVector &zz_k, Matrix &zzp_k,
                                           ColumnVector &z_k, ColumnVector &x_post, SymmetricMatrix &p_post)
    {
        //创建 cross correlation 矩阵： XZ（nxm）
        Matrix xzCross = Matrix(stateCount, measurementCount);

        xzCross = 0.0;
        for (int col = 1; col <= sigmaPointsCount; col++) //2n+1 个 simga points
        {
            //系统状态值向量：X state difference
            ColumnVector x_diff = xsig_pred.columnCopy(col) - x_k;
            //定义一个(stateCount, 1)矩阵，下面XZ的计算必须用矩阵，ColumnVector不可用
            Matrix x_diffMatrix(stateCount, 1);
            for (int row = 1; row <= stateCount; row++)
                x_diffMatrix(row, 1) = x_diff(row);

            //测量值向量：Z difference
            ColumnVector z_diff = zsig_pred.columnCopy(col) - zz_k;
            //定义一个(measurementCount, 1)矩阵，下面XZ的计算必须用矩阵，ColumnVector不可用
            Matrix z_diffMatrix(measurementCount, 1);
            for (int row = 1; row <= measurementCount; row++)
                z_diffMatrix(row, 1) = z_diff(row);

            xzCross = xzCross + (Matrix)(weightVector(col /*row*/) * x_diffMatrix) * z_diffMatrix.transpose();
        }

        //卡尔曼增益（gain）: (nxm)
        //Note:  zzp_k 这里建议都采用Matrix类型，因为掺杂着SymmetricMatrix类型会使对矩阵就逆矩阵（inverse()）时，运算结果不正确
        Matrix K_gain = xzCross * zzp_k.inverse(); //XZ(nxm) * zzp_k(mxm) = (nxm)

        //传感器测量值 - 预测测量值： Z_diff
        ColumnVector Z_diff = z_k - zz_k;

        //update state mean and covariance matrix
        x_post = x_k + K_gain * Z_diff;
        p_post = p_k - (SymmetricMatrix)(K_gain * (Matrix)zzp_k * K_gain.transpose()); // (nxm) * (m*m)  * (m*n)
    }


    //为了模拟测试，我们需要利用飞机的运动学模型生成一些模拟的测量数据：测量值列向量Z_k: (d_gps,h_gps,p_bp,v_speed)  m=4
    void UKF::GenarateDemoMeasurementData()
    {
        double initVelocity = 70; // m/s
        double Acceleration = 20; // m/s^2
        double pitch = 30;      //角度值： °

        double d_measurement[60];
        double h_measurement[60];
        double h_measurement_barometers[60];
        double v_measurement[60];

        double bp_measurement_barometers[60];//大气压

        //根据初中物理运动学公式:
        //vt = v0 + a *Δt
        //st = s0 + v0 * Δt + 0.5 * a * (Δt) ^ 2

        double d = 0, h = 0, v = initVelocity;
        float deltaT = 0.5;

        int noiseForGpsD,noiseForGpsH,noiseForBpH,noiseForSpeedometerV;

        for( int i = 0; i < 60; i++ ) // 模拟预测和更新周期是2， 即每秒2执行2次。  30秒执行60次
        {
            //x_k = x_k-1 + 0 + cos(30°) * v_k-1 * Δt + 0.5 * cos(30°) * a * (Δt) ^ 2
            d = d + std::cos(30 * PI / 180.0f) * v * deltaT + 0.5 * std::cos(30 * PI / 180.0f) * Acceleration * std::pow(deltaT, 2);
            //h_k = 0 + h_k-1 + sin(30°) * v_k-1 * Δt + 0.5 * sin(30°) * a * (Δt) ^ 2
            h = h + std::sin(30 * PI / 180.0f) * v * deltaT + 0.5 * std::sin(30 * PI / 180.0f) * Acceleration * std::pow(deltaT, 2);
            //v_k = 0 + 0 + v_k-1 + a * Δt
            v = v + Acceleration * deltaT;

            noiseForGpsD = std::rand()%60;  //生成模拟数据时,gps测量水平误差+/-30m
            noiseForGpsH = std::rand()%60;  //生成模拟数据时,gps测量水平误差+/-30m
            noiseForBpH = std::rand()%200;  //生成模拟数据时,气压计测量误差+/-100m
            noiseForSpeedometerV = std::rand()%6;  //生成模拟数据时,速度计测量误差+/-3m/s

            d_measurement[i] = d + ((noiseForGpsD > 30) ? noiseForGpsD-30 : -noiseForGpsD);
            h_measurement[i] = h + ((noiseForGpsH > 30) ? noiseForGpsH-30 : -noiseForGpsH);
            h_measurement_barometers[i] = h + ((noiseForGpsH > 100) ? noiseForGpsH-100 : -noiseForGpsH);
            v_measurement[i] = v + ((noiseForSpeedometerV > 3) ? noiseForSpeedometerV-3 : -noiseForSpeedometerV);

            bp_measurement_barometers[i] = 101.325 * std::pow((1 - h_measurement_barometers[i] / 44300), 5.256);
        }

        cout << "GenarateDemoMeasurementData: d_measurement= [";
        for (unsigned int i = 0; i < 60; i++)
            cout << d_measurement[i]<<",";
        cout << "]\n\n";
        cout << "GenarateDemoMeasurementData: h_measurement= [";
        for (unsigned int i = 0; i < 60; i++)
            cout << h_measurement[i] << ",";
        cout << "]\n\n";
        cout << "GenarateDemoMeasurementData: h_measurement_barometers= [";
        for (unsigned int i = 0; i < 60; i++)
            cout << h_measurement_barometers[i] << ",";
        cout << "]\n\n";
        cout << "GenarateDemoMeasurementData: v_measurement= [";
        for (unsigned int i = 0; i < 60; i++)
            cout << v_measurement[i] << ",";
        cout << "]\n\n";

         cout << "GenarateDemoMeasurementData: bp_measurement_barometers= [";
        for (unsigned int i = 0; i < 60; i++)
            cout << bp_measurement_barometers[i] << ",";
        cout << "]\n\n";
    }
}